#include <iostream>

#include "astar-pathfinder.h"

Astar_Pathfinder::Astar_Pathfinder(Map *map, int x1, int y1, int x2, int y2)
{
	MAP = map;
	X1 = x1;
	X2 = x2;
	Y1 = y1;
	Y2 = y2;
	CAMEFROM = new Double_Linked_List();
	CLOSED = new Double_Linked_List();
	OPEN = new Double_Linked_List();
	
	if (!generatePath())
		CAMEFROM = NULL;
}

bool Astar_Pathfinder::generatePath()
{
	int i, j, yx, yy, tentative_g_cost;
	Node *tempa, *tempb;
	Object *object;
	Terrain *n, *x, *y, *start, *goal;
     	// The set of nodes already evaluated.
//     closedset := the empty set
     	// The set of tentative nodes to be evaluated.
//    openset := set containing the initial node
     	// The map of navigated nodes.
//     came_from := the empty map
     // Distance from start along optimal path.
//     g_score[start] := 0
//     h_score[start] := heuristic_estimate_of_distance(start, goal)
	object = MAP->isAt(X1, Y1);
	if (object->isTerrain())
		start = (Terrain *) object;
	else
		std::cerr << "Error in Astar_Pathfinder::generatePath(): object is not terrain!" << std::endl;
	object = MAP->isAt(X2, Y2);
	if (object->isTerrain())
		goal = (Terrain *) object;
	else
		std::cerr << "Error in Astar_Pathfinder::generatePath(): object is not terrain!" << std::endl;
	start->setGCost(0);
	start->setHCost(estimateDistance(X1, Y1, X2, Y2));
     // Estimated total distance from start to goal through y.
 //    f_score[start] := h_score[start]
 
 	// Add starting position to OPEN list.
 	OPEN->add((Object *)start);
 
 	bool tentative_is_better;
	while (!OPEN->isEmpty())
     	{
     		tempa = OPEN->node(0);
     		x = (Terrain *) tempa->object();
 //       	x := the object in openset having the lowest f_score[] value
 		for (i=1; i<OPEN->size(); i++)
 		{
 			tempb = OPEN->node(i);
 			n = (Terrain *) tempb->object();
 			if (n->fCost() < x->fCost())
 			{
 				x = n;
 				tempa = tempb;
 			}
 				
 		}
         	if (x == goal)
         	{
 //             	reconstructPath(came_from, came_from[goal]);
              		return true;
              	}
 //        	remove x from openset
 		OPEN->remove((Object *)x);
 //       	add x to closedset
 		CLOSED->add((Object *)x);
 		
 //        	foreach y in neighbor_nodes(x)
 		for (i=-1; i<=1; i++)
         	{
         		for (j=-1; j<=1; j++)
         		{
         			yx = x->x() + j;
         			yy = y->y() + i;
         			if (((i == 0 ) && ( j == 0)) || (yx < 0) || (yx >= MAP->width()) || (yy < 0) || (yy >= MAP->height()))
         				continue;
         			y = (Terrain *) MAP->isAt(j, i);
         			
//	            		if y in closedset
				if (CLOSED->contains(y))
				{
  	              			continue;
  	              		}
				tentative_g_cost = x->gCost() + estimateDistance(x->x(), x->y(), y->x(), y->y());
 
//            	 		if y not in openset
				if (!OPEN->contains((Object *)y))
             			{
 					OPEN->add(y);
 					tentative_is_better = true;
             			}
             			else if (tentative_g_cost < y->gCost())
                 			tentative_is_better = true;
             			else
                 			tentative_is_better = false;
                 
                 
             			if (tentative_is_better == true)
             			{
//                 			came_from[y] := x
					CAMEFROM->add(x);
 
 //                			g_score[y] := tentative_g_score
 					y->setGCost(tentative_g_cost);
                 
 //                			h_score[y] := heuristic_estimate_of_distance(y, goal)
 					y->setHCost(estimateDistance(y->x(), y->y(), goal->x(), goal->y()));
 //                			f_score[y] := g_score[y] + h_score[y]
 //                			Update(closedset,y)
 //                			Update(openset,y)
                 		}
                 	}
                 }
	}
 
     	return false;
}

// This function returns a pointer to CAMEFROM, the list containing the path.
Double_Linked_List *Astar_Pathfinder::getPath()
{
	return CAMEFROM;
}

// This function estimates the distance from one point to another in Manhattan units.
int Astar_Pathfinder::estimateDistance(int x1, int y1, int x2, int y2)
{
	return (x2 - x1) + (y2 - y1);
}
/* 
void Astar_Pathfinder::reconstructPath(came_from, current_node)
{
     if came_from[current_node] is set
         p = reconstruct_path(came_from, came_from[current_node])
         return (p + current_node)
     else
         return current_node
}
*/
